/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2018 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_ATTACHEDSENSORCONVERTERCONFIGURATION_H_
#define __MMM_ATTACHEDSENSORCONVERTERCONFIGURATION_H_

#include "../ApplicationBaseConfiguration.h"

#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <string>
#include <MMM/XMLTools.h>
#include <VirtualRobot/RuntimeEnvironment.h>

/*!
    Configuration of MMM attached sensor converter.
*/
class MMMAttachedSensorConverterConfiguration : public ApplicationBaseConfiguration
{
public:
    MMMAttachedSensorConverterConfiguration(std::vector<int> initIndexes = std::vector<int>(), std::string initSplitMeasurementByDelimiter = std::string(), std::string initSplitMeasurementByRegex = std::string()) :
        ApplicationBaseConfiguration(),
        offset(0.0f),
        splitMeasurementByDelimiter(initSplitMeasurementByDelimiter),
        splitMeasurementByRegex(initSplitMeasurementByRegex),
        byRegex(false),
        indexes(initIndexes)
    {
    }

    std::string inputMotionPath;
    std::string motionName;
    std::string dataFilePath;
    std::string sensorConfigurationFilePath;
    float offset; // for Synchronization
    std::string splitMeasurements;
    std::string splitMeasurementByDelimiter;
    std::string splitMeasurementByRegex;
    bool ignoreFirstLine;
    bool byRegex;
    std::string timestamp;
    std::vector<int> indexes;
    std::string sensorName;
    std::string sensorDescription;
    std::vector<std::string> sensorPluginPaths;
    std::string outputMotionPath;

    bool processCommandLine(int argc, char *argv[])
    {
        VirtualRobot::RuntimeEnvironment::considerKey("inputMotion");
        VirtualRobot::RuntimeEnvironment::considerKey("motionName");
        VirtualRobot::RuntimeEnvironment::considerKey("dataFile");
        VirtualRobot::RuntimeEnvironment::considerKey("sensorConfigurationFile");
        VirtualRobot::RuntimeEnvironment::considerKey("ignoreFirstLine");
        VirtualRobot::RuntimeEnvironment::considerKey("splitMeasurements");
        VirtualRobot::RuntimeEnvironment::considerKey("splitMeasurementByRegex");
        VirtualRobot::RuntimeEnvironment::considerKey("splitMeasurementByDelimiter");
        VirtualRobot::RuntimeEnvironment::considerKey("indexes");
        VirtualRobot::RuntimeEnvironment::considerKey("timestamp");
        VirtualRobot::RuntimeEnvironment::considerKey("sensorName");
        VirtualRobot::RuntimeEnvironment::considerKey("sensorDescription");
        VirtualRobot::RuntimeEnvironment::considerKey("sensorPlugins");
        VirtualRobot::RuntimeEnvironment::considerKey("outputMotion");
        VirtualRobot::RuntimeEnvironment::considerKey("timestepDelta");
        VirtualRobot::RuntimeEnvironment::processCommandLine(argc,argv);
        VirtualRobot::RuntimeEnvironment::print();

        inputMotionPath = getParameter("inputMotion", true, true);
        motionName = getParameter("motionName", false, false);
        dataFilePath = getParameter("dataFile", true, true);
        ignoreFirstLine = VirtualRobot::RuntimeEnvironment::hasValue("ignoreFirstLine");
        splitMeasurements = getParameter("splitMeasurements");
        if (splitMeasurements.empty()) splitMeasurements = "\n";
        splitMeasurementByRegex = getParameter("splitMeasurementByRegex");
        splitMeasurementByDelimiter = getParameter("splitMeasurementByDelimiter");
        if (!splitMeasurementByDelimiter.empty()) byRegex = false;
        else if (!splitMeasurementByRegex.empty()) byRegex = true;
        else {
            MMM_ERROR << "Either splitMeasurementsByRegex or splitMeasurementsByDelimiter need to be used." << std::endl;
            valid = false;
        }

        std::string indexes = getParameter("indexes");
        if (!indexes.empty()) {
            this->indexes = std::vector<int>();
            std::vector<std::string> splittedIndexes;
            boost::split(splittedIndexes, indexes, boost::is_any_of(";"));
            for (const std::string &i : splittedIndexes) {
                try {
                    indexes.push_back(MMM::XML::convertTo<int>(i, "Index " + i + " is not of type integer!"));
                } catch (MMM::Exception::MMMException &e) {
                    MMM_ERROR << e.what() << std::endl;
                    valid = false;
                }
            }
        }
        timestamp = getParameter("timestamp");
        sensorConfigurationFilePath = getParameter("sensorConfigurationFile", true, true);
        sensorName = getParameter("sensorName", false, false);
        sensorDescription = getParameter("sensorDescription", false, false);
        sensorPluginPaths = getParameters("sensorPlugins");
        outputMotionPath = getParameter("outputMotion", false, false);

        std::string offsetString = getParameter("timestepDelta", false, false);
        if (!offsetString.empty()) offset = MMM::XML::convertToFloat(offsetString.c_str());

        return valid;
    }

    void print()
    {
        MMM_INFO << "*** MMMAttachedSensorConverter Configuration ***" << std::endl;
        std::cout << "Input motion: " << inputMotionPath << std::endl;
        std::cout << "Motion name: " << motionName << std::endl;
        std::cout << "Data file:" << dataFilePath << std::endl;
        std::cout << "Sensor configuration file:" << sensorConfigurationFilePath << std::endl;
        std::cout << "Output motion: " << outputMotionPath << std::endl;
    }
};

#include <MMM/MMMCore.h>
#include <MMM/Motion/Motion.h>
#include <MMM/Motion/MotionReaderXML.h>
#include <MMM/Motion/MotionWriterXML.h>
#include "MMMAttachedSensorConverterConfiguration.h"
#include "AddAttachedSensorConfiguration.h"

int convert(int argc, char *argv[], MMM::BaseAddAttachedSensorConfigurationPtr sensorConfiguration, MMMAttachedSensorConverterConfiguration* converterConfiguration = new MMMAttachedSensorConverterConfiguration())
{
    if (!converterConfiguration->processCommandLine(argc, argv)) {
        MMM_ERROR << "Error while processing command line, aborting..." << std::endl;
        return -1;
    }

    MMM::MotionList motions;
    MMM::MotionPtr motion;
    try {
        MMM_INFO << "Reading motion file '" << converterConfiguration->inputMotionPath << "'!" << std::endl;
        MMM::MotionReaderXMLPtr motionReader(new MMM::MotionReaderXML(converterConfiguration->sensorPluginPaths));
        motions  = motionReader->loadAllMotions(converterConfiguration->inputMotionPath);
        motion = MMM::Motion::getMotion(motions, converterConfiguration->motionName);

        MMM_INFO << "Reading sensor configuration file file '" << converterConfiguration->sensorConfigurationFilePath << "'!" << std::endl;
        MMM::RapidXMLReaderNodePtr configuration = MMM::RapidXMLReader::FromFile(converterConfiguration->sensorConfigurationFilePath)->getRoot();
        sensorConfiguration->loadSensorConfiguration(configuration);
        sensorConfiguration->setName(converterConfiguration->sensorName);
        sensorConfiguration->setDescription(converterConfiguration->sensorDescription);

        MMM_INFO << "Reading sensor data file '" << converterConfiguration->dataFilePath << "'!" << std::endl;
        std::string sensorFile;
        std::ifstream ifs(converterConfiguration->dataFilePath);
        sensorFile.assign(std::istreambuf_iterator<char>(ifs), std::istreambuf_iterator<char>());

        MMM_INFO << "Converting sensor measurements!" << std::endl;
        sensorConfiguration->addSensorMeasurements(sensorFile, converterConfiguration->ignoreFirstLine, converterConfiguration->splitMeasurements, converterConfiguration->byRegex,
                                                   converterConfiguration->splitMeasurementByDelimiter, converterConfiguration->splitMeasurementByRegex, converterConfiguration->indexes, converterConfiguration->timestamp);

        MMM_INFO << "Adding sensor to motion!" << std::endl;
        motion->addSensor(sensorConfiguration->getSensor(), converterConfiguration->offset);
    }
    catch (MMM::Exception::MMMException &e) {
        MMM_ERROR << e.what() << std::endl;
        return -2;
    }

    MMM_INFO << "Writing motions to " << converterConfiguration->outputMotionPath << std::endl;
    MMM::MotionWriterXML::writeMotion(motions, converterConfiguration->outputMotionPath);

    return 0;
}


#endif
